#include "main.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "bmi088.h"
#include "ist8310driver.h"
#include "usart.h"
#include "runge_kutta.h"

#include "start.h"

#define TASK_PERIOD pdMS_TO_TICKS(10) // 设置周期为1ms



vofa_buff_t vofa;

void imuTask(void const *argument)
{
    vofa.tail[0] = 0x00;
    vofa.tail[1] = 0x00;
    vofa.tail[2] = 0x80;
    vofa.tail[3] = 0x7f;

    uint16_t heartbeat = 0;

    BMI088_INIT();
    ist8310_init();

    TickType_t xLastWakeTime;
    xLastWakeTime = xTaskGetTickCount(); // 初始化上次唤醒时间为当前时间

    vTaskDelay(10); // 等待地磁计启动
    while (1)
    {
        /*读取原数据*/
        ReadAccData(&BMI088_Struct.acc_data.acc_raw_data);
        ReadGyroData(&BMI088_Struct.gyro_data.gyro_raw_data);
        ist8310_read_mag(magnetism);                                
        magnetism[0] = (magnetism[0] + 13.358);                      //椭球拟合矫正数据
        magnetism[1] = ((magnetism[1] + 14.349) * 56.972) / 50.405;  //椭球拟合矫正数据
        magnetism[2] = ((magnetism[2] - 0.15413) * 56.972) / 46.276; //椭球拟合矫正数据

        imu.quat = updateAttitude(imu.quat, BMI088_Struct.acc_data.acc_raw_data.x, BMI088_Struct.acc_data.acc_raw_data.y, BMI088_Struct.acc_data.acc_raw_data.z,
                                  BMI088_Struct.gyro_data.gyro_raw_data.roll, BMI088_Struct.gyro_data.gyro_raw_data.pitch, BMI088_Struct.gyro_data.gyro_raw_data.yaw,
                                  -magnetism[0], -magnetism[1], -magnetism[2], 0.01);

        vofa.buff[0] = imu.quat.q0;
        vofa.buff[1] = imu.quat.q1;
        vofa.buff[2] = imu.quat.q2;
        vofa.buff[3] = imu.quat.q3;

        vofa.buff[4] = magnetism[0];
        vofa.buff[5] = magnetism[1];
        vofa.buff[6] = magnetism[2];




        HAL_UART_Transmit(&huart6, (uint8_t *)&vofa, 4 * 11, 1000);
        HAL_UART_Transmit(&huart6, (uint8_t *)&vofa.tail, 4, 1000);

        /* heartbeat init*/
        heartbeat++;
        if (heartbeat == 1000)
        {
            heartbeat = 1;
        }
        /*Task code end*/
        vTaskDelayUntil(&xLastWakeTime, TASK_PERIOD); // 绝对延时
    }
}